#include "HeadTracker.h"

HeadTracker::HeadTracker()
{

    vidGrabber.listDevices();
    vidGrabber.setDeviceID(0);
    vidGrabber.setVerbose(true);
    vidGrabber.initGrabber(320,240);


    colorImg.allocate(320,240);
	grayImage.allocate(320,240);
	grayBg.allocate(320,240);
	grayDiff.allocate(320,240);
	bLearnBakground = true;
	threshold = 80;

    face_pos_x = 0;
    face_pos_y = 0;
	//lets load in our face xml file
	haarFinder.setup("data/haarXML/haarcascade_frontalface_default.xml");

}

HeadTracker::~HeadTracker()
{
    //dtor
}


void HeadTracker::update(){

    bool bNewFrame = false;

    vidGrabber.grabFrame();
    bNewFrame = vidGrabber.isFrameNew();

	if (bNewFrame){

        colorImg.setFromPixels(vidGrabber.getPixels(), vidGrabber.getWidth(),vidGrabber.getHeight());

        grayImage = colorImg;
		if (bLearnBakground == true){
			grayBg = grayImage;		// the = sign copys the pixels from grayImage into grayBg (operator overloading)
			bLearnBakground = false;
		}

		haarFinder.findHaarObjects(grayImage, 10, 99999999, 10);
	}

    colorImg.draw(20,20);
	int numFace = haarFinder.blobs.size();

	int imax=-1;
	float big_face=0.0;
	for(int i = 0; i < numFace; i++){

	    if(big_face<haarFinder.blobs[i].boundingRect.width+haarFinder.blobs[i].boundingRect.height){
            big_face = haarFinder.blobs[i].boundingRect.width+haarFinder.blobs[i].boundingRect.height;
            imax = i;
        }

		float cx = haarFinder.blobs[i].centroid.x;
		float cy = haarFinder.blobs[i].centroid.y;

	}

    if(imax>=0){
        face_pos_x = -(haarFinder.blobs[imax].centroid.x-320/2);
        face_pos_y = haarFinder.blobs[imax].centroid.y-240/2;
    }

}
